/***************************************************************
*
* M3D_IK.CPP
*
* Inverse kinematics math (2D & 3D)
***************************************************************/
#include <m3d/m3d_stdafx.h>

#include <stdio.h>

#include <ap_comm.h>
#include <m3d.h>

/***************************************************************
* m3dIKSolve2D()
*
* IN:  l1, l2 - length of two joints
*      a, b   - coordinates of the end effector
* OUT: x, y   - coordinates of the middle joint
* RET: FALSE  - l1+l2 < len(a,b); joints are fully stretched
*      TRUE   - otherwise
*
* NOTE:
*  1. Only one solution of the kinematics is returned.
*     Need to be improved in the future.
*  2. In case the problem cannot be resolved (l1+l2 < len(a,b))
*     the joints are fully stretched, and FALSE is returned
***************************************************************/
BOOL m3dIKSolve2D (float l1, float l2, float a, float b, float *x, float *y)
{
   float   c1, c2;
   float   x1, x2, y1, y2;
   float   t;
   int     n;

   if (m3dIsZero_A(a, 0.0001f)) {
      ASSERT(!m3dIsZero_A(b, 0.0001f));
      c1 = (l1*l1 - l2*l2 + a*a + b*b) / (2*b);
      c2 = a / b;

      n = m3dSolveQuadraticEq(1+c2*c2, -2*c1*c2, c1*c1-l1*l1, &x1, &x2);
      if (n > 0) {
         *x = x2;
         *y = c1 - c2*(*x);
         return TRUE;
      }
   } else {
      c1 = (l1*l1 - l2*l2 + a*a + b*b) / (2*a);
      c2 = b / a;

      n = m3dSolveQuadraticEq(1+c2*c2, -2*c1*c2, c1*c1-l1*l1, &y1, &y2);
      if (n > 0) {
         *y = y2;
         *x = c1 - c2*(*y);
      }
      return TRUE;
   }

   ASSERT(n == 0);
   t = l1/m3dSqrt(a*a+b*b);
   *x = a*t;
   *y = b*t;
   return FALSE;
}

/***************************************************************
* m3dIKSolve3D()
*
* IN:  l1, l2 - length of two joints
*      p1     - fixed point of the IK chain
*      p2     - end effector
*      pole   - point that forms a plane with 'p1', 'p2', where
*               the joint 'p3' resides
* OUT: p3     - coordinates of the middle joint
* RET: FALSE  - l1+l2 < dist(p1,p2); joints are fully stretched
*      TRUE   - otherwise
***************************************************************/
BOOL m3dIKSolve3D (float l1, float l2, m3dV *p1, m3dV *p2, m3dV *pole, m3dV *p3)
{
   m3dV      p, v;
   m3dV      vy, vz;
   m3dMATR   matr, matrInv;
   BOOL      rc;

   m3dSubtractVector(p2, p1, &vz);
   m3dGetPlaneNormal(pole, p1, p2, &vy);

   m3dNormalize(&vy);
   m3dNormalize(&vz);
   matr.MakeLCS2WCS(p1, NULL, &vy, &vz);

   // rendRenderCS(dcCameraPtr, &matr, 3.f);

   matr.Invert(&matrInv);

   matrInv.TransformPoint(p2, &p);
   ASSERT(m3dIsZero_A(p.y, 0.0001f));

   rc = m3dIKSolve2D(l1, l2, p.x, p.z, &v.x, &v.z);
   v.y = 0.f;

   matr.TransformPoint(&v, p3);
   
#ifdef _DEBUG
   {
      // make sure the joint lies in the plane with IK effector,
      // IK origin, and IK pole
      m3dPOLY poly;
      float   dist;

      poly.nVert = 3;
      poly.vert[0] = *p1;
      poly.vert[1] = *p2;
      poly.vert[2] = *pole;
      dist = m3dDistPointPlane(p3, &poly);
      ASSERT(m3dFAbs(dist) < 0.001f);
   }
#endif

   return rc;
}

//
// End-of-flie M3D_IK.CPP
//
